/**
  MOMD project, Anyang Normal University, IMP-CAS
  \class ETFMath
  \brief Math class, to provide some general math methods.
  \author SUN Yazhou, aisa.rabbit@163.com
  \date Created: 2020/07/09
  \date Last modified: 2023/10/18 by SUN Yazhou
  \copyright 2020-2023 SUN Yazhou
  \copyright Copyright (c) 2021-2024 Anyang Normal U. && IMP-CAS with LGPLv3 LICENSE
  \copyright MOMD project, Anyang Normal University, IMP-CAS
*/

#include <iostream>
#include <cstring>
#include <algorithm>
#include <complex> // complex number operation
#include <utility>
#include <unistd.h> // for access
#include <cstdio>
#include "config.h"
#ifdef _CATCH_
#include "catch.h"
#endif
#include <array>
#include "ETFMath.h"
#include "ETFMsg.h"
#include "ETFDetectorManager.h"
#include "ETFTrack.h"
#include "ETFTrackTa.h"
#include "ETFDCHit.h"
#include <filesystem> // for getting file size

using std::swap;
using std::max;
using std::cout;
using std::endl;
using std::array;
using std::complex;
typedef complex<double> cdouble;

#define er ETFMsg::Error
// #define DEBUG

// the z border of the effective magField (in mm)
const double ETFMath::kzMagIn = -555.5;
const double ETFMath::kzMagOut = 555.5;
// offset due to the laser ball connector pat
// 29.12: measured on Dec.11, 2023, using 25.00mm pat
const double ETFMath::DPAT = 29.12; // 28.79, 29.3, 31.16;

// r_global = R.r_local
// angle0: yaw, angle1: pitch, angle2: roll, intrinsic rotation; (y-x'-z")
// R(yaw, pitch, roll) = Ry(yaw).Rx'(pitch).Rz"(roll)
void ETFMath::rotate(double *pOut, const double *pIn, const double *angIn){
  const double s0 = sin(angIn[0]), s1 = sin(angIn[1]), s2 = sin(angIn[2]);
  const double c0 = cos(angIn[0]), c1 = cos(angIn[1]), c2 = cos(angIn[2]);

  pOut[0] = (c0*c2+s0*s1*s2)* pIn[0] +(c2*s0*s1-c0*s2)* pIn[1] +c1*s0* pIn[2];
  pOut[1] = c1*s2*            pIn[0] +c1*c2*            pIn[1] -s1*    pIn[2];
  pOut[2] = (c0*s1*s2-c2*s0)* pIn[0] +(c0*c2*s1+s0*s2)* pIn[1] +c0*c1* pIn[2];
} // end member function rotate

// print array in an orderly fashion
void ETFMath::peek(int *v, int len){
  for(int i = 0; i < len; i++){
    cout << v[i] << '\t';
    if(i != 0 && i % 10 == 0) cout << endl;
  } // end for
} // end member function Inspect

// returns the value ln[Gamma(xx)] for xx > 0.
double ETFMath::gammaln(double xx){
  if(xx <= 0.) er("ETFMath", "gammaln: x > 0. is mandatory.");
  // internal arithmetic will be done in double precision, a nicety that you can
  // omit if five-figure accuracy is good enough
  static const double c[6] = {76.18009172947146,-86.50532032941677,
    24.01409824083091,-1.231739572450155,
    0.1208650973866179e-2,-0.5395239384953e-5};
  double tmp = xx + 5.5, x = xx; // (z+gamma+1/2), gamma = 5, N = 6
  tmp -= (x+0.5)*log(tmp);
  double ser = 1.000000000190015; // the additive series
  for(int i = 0; i < 6; i++) ser += c[i]/++x;
  return -tmp+log(2.5066282746310005*ser/xx); // sqrt(2pi)
} // end member function gammaln

// returns ln(n!)
double ETFMath::factln(int n){
  static double a[101]; // a static array is automatically initialized to zero

  if(n < 0) er("ETFMath", "factln: n is negative.");
  if(n <= 1) return 0.;
  if(n <= 100) return a[n] ? a[n] : (a[n]=gammaln(n+1.));
  return gammaln(n+1.); // out of range of table
} // end member function factln

// \retval n!
double ETFMath::Factorial(int n){
  static int ntop = 4;
  static const int NMAX = 101; // size of the cached table, max=171 for double
  static const int NMAX1 = NMAX - 1;
  static double a[NMAX] = {1., 1., 2., 6., 24.}; // fill in table only as required

  if(n < 0) er("ETFMath", "Factorial: n: %d is minus", n);
  // larger value than size of the table is required. Actually, this big a value
  // is going to overflow on many computers, but no harm in trying
  if(n > NMAX1) return exp(gammaln(n+1.));
  while(ntop < n){
    ntop++;
    a[ntop] = a[ntop-1]*ntop;
  } // end while
  return floor(0.5+a[n]); // clear off the roundoff error
} // end member function Factorial

// returns Binomial coefficients
int ETFMath::Binomial(int n, int m){
  if(m > n)
    er("ETFMath",
      "Binomial: n: %d is larger than m: %d!", n, m);

  // the floor function cleans up roundoff error for smaller values of n and m
  return floor(0.5+exp(factln(n)-factln(m)-factln(n-m)));
} // end member function Binomial

double ETFMath::Beta(double z, double w){
  if(z < 0. || w < 0.) er("ETFMath", "Beta: z or z is minus");
  return exp(gammaln(z)+gammaln(w)-gammaln(z+w));
} // end member function Beta

// \retval n!!
int ETFMath::BiFactorial(int n){
  return n <= 1 ? 1 : n * BiFactorial(n-2);
} // end member function BiFactorial

// the incomplete gamma function gammap, gammap(a,x)=P(a,x)=gamma(a,x)/Gamma(a)
double ETFMath::gammap(double a, double x){
  if(x < 0. || a <= 0.) er("ETFMath", "gammap: invalid arguements");

  if(x < (a+1.)) return gser(a, x); // series representation
  return 1. - gcf(a, x);
} // end member function gammap
// the incomplete gamma function gammaq, gammaq(a,x)=Q(a,x)=1-P(a,x)
double ETFMath::gammaq(double a, double x){
  if(x < 0. || a <= 0.) er("ETFMath", "gammaq: invalid arguements");

  if(x < (a+1.)) return 1. - gser(a, x); // continued fraction representation
  return gcf(a, x);
} // end member function gammaq

#ifdef _CATCH_
TEST_CASE("Incomplete Gamma function", "[gammap]"){
  // CHECK(ETFMath::gammap(1., 0.5) == Approx(0.606531).epsilon(1.e-8));
  // CHECK(ETFMath::gammaq(1., 0.5) == Approx(1.-0.606531).epsilon(1.e-8));
  CHECK(ETFMath::gammap(1., 0.5) == Approx(0.3934693402873666).epsilon(1.e-8));
  CHECK(ETFMath::gammap(1., 0.1) == Approx(0.09516258196404048).epsilon(1.e-8));
  CHECK(ETFMath::gammaq(1., 3.5) == Approx(0.0301973834223185).epsilon(1.e-8));
  CHECK(ETFMath::gammaq(1., 4.1) == Approx(0.016572675401761255).epsilon(1.e-8));
  CHECK(exp(ETFMath::gammaln(0.5)) == Approx(sqrt(ETFMath::Pi())).epsilon(1.e-8));
} // end TEST_CASE
#endif

// returns the incomplete gamma function P(a,x) evaluated by its series
// representation. Optionally returns ln[Gamma(a)] as gln.
double ETFMath::gser(double a, double x, double *gln){
  static const double ITMAX = 100; // maximum iteration times
  static const double EPS = 3.e-7; // fractional accuracy

  if(x < 0.) er("ETFMath", "gser: x < 0., x: %f", x);
  double ap = a, sum, item = sum = 1./a;
  for(int i = 0; i < ITMAX; i++){
    sum += (item *= x/++ap);
    if(fabs(item) < fabs(sum)*EPS)
      return sum * exp(-x + a*log(x) - (gln ? *gln=gammaln(a) : gammaln(a)));
  } // end for over i
  er("ETFMath", "gser: a too large, and IMAX too small.");
  return 0.; // never gets here
} // end member function gamser

// returns the incomplete gamma function Q(a,x) evaluated by its continued
// fraction representation. Optionally returns ln[Gamma(a)] as gln.
double ETFMath::gcf(double a, double x, double *gln){
  static const double ITMAX = 100; // maximum iteration times
  static const double EPS = 3.e-7; // factional accuracy
  // number near the smallest representable floating-point number
  static const double FPMIN = 1.e-30; // modified Lentz's algorithm

  // set up for evaluating continued fraction by modified Lentz's method with b0=0.
  // b1 = x+1.-a, c1 = \infty (since c0 = 0), d1 = 1/b1, fr1=b0+a1/b1=a1/b1=1/b1=d1
  double b = x+1.-a, c = 1./FPMIN, d = 1./b, fr = d;
  for(int i = 1; i < ITMAX; i++){
    double aip = -i*(i-a); // a_{i+1}
    b += 2.; d = aip*d+b; // b2, d2 = 1/(a2*d1+b2)
    if(fabs(d) < FPMIN) d = FPMIN;
    c = b+aip/c; // c2 = b2 + a2/c1
    if(fabs(c) < FPMIN) c = FPMIN;
    d = 1./d;
    double delta = c*d;
    fr *= delta; // fr_2 = fr_1*c2*d2
    if(fabs(delta-1.) < EPS)
      return exp(-x+a*log(x)-(gln ? *gln = gammaln(a) : gammaln(a)))*fr; // put factors in front
  } // end for over i
  er("ETFMath", "gcf: a too large, and IMAX too small.");
  return 0.; // never gets here
} // end member function gcf

// fit the track and assign slope and intercept
double ETFMath::IterFit(const int np, const double *z, const double *x,
    const double *r, double &kL, double &bL, double d2ex){ // external d2
  // const int n = pow(2, np); // nof iterations
  const int n = 1 << np; // nof iterations (nof sign arrangements)
  double d2, kl, bl, d2min = d2ex;
  // dx not unkown, so set to minus deliberately
  vector<double> dx; dx.resize(np); dx.assign(np, -1.);
  // temporarily determine kL and bL using only wire positions
  if(d2min > 1.e10) // not assigned
    LinearFit(z, x, dx.data(), np, bL, kL, d2min);
  // d2min += fabs(kL)*100.; // we'd like it to be as small as possible
  vector<double> zt, xt;
  zt.resize(np); xt.resize(np);
  zt.assign(np, -999.); xt.assign(np, -999.);
  const double cosTheta = 1./ sqrt(1.+kL*kL), sinTheta = kL*cosTheta;
  for(int i = n; i--;){
    for(int j = 0; j < np; j++){
      // the bit representation of i gives the sign composition of all the hits //
      // const double R = (2*((i>>j) & 0x1)-1) * r[j]; // test the j-th bit of i: 0->-r, 1->r
      const double R = i>>j & 0x1 ? r[j] : -r[j]; // test the j-th bit of i: 0->-r, 1->r
      zt[j] = z[j] + R*sinTheta;
      xt[j] = x[j] - R*cosTheta;
    } // end for over j
    LinearFit(zt.data(), xt.data(), dx.data(), np, bl, kl, d2);
    // d2 += fabs(kl)*100.; // we'd like it to be as small as possible
    if(d2 < d2min){ kL = kl; bL = bl; d2min = d2; }
  } // end for over i

  return d2min;
} // end member function IterFit

// iterative fit for united tracking of preTaTrk and postTaTrk. For X1-X2 bothly
// fired cases, their signs of drift distances are determined so that the track goes
// in between, and only the drift distance signs of the singly fired hits are iterated
// hence the name of IterFit4Single
// l: k3x+b3: postMagTrk
double ETFMath::IterFit4Single(ETFTrackTa *t0, ETFTrackTa *t1, const double k3,
    const double b3, double &fDxTa, double &fDx2){
  const int nh[2] = {t0->GetNofHits(), t1->GetNofHits()};
  int nSingle[2]{}; // number of singly fired layers
  for(int i = 2; i--;) if(3 == nh[i]) nSingle[i] = 1;
  // const int n = pow(2, nSingle[0]+nSingle[1]); // count of iterations
  const int n = 1 << (nSingle[0]+nSingle[1]); // count of iterations
  double kl[3]{}, bl[3]{}, d2[2]{};
  kl[2] = k3; bl[2] = b3; // postMagTrk's parameters
  double d2Tmin = 1.e200, d2T; // T: total
  double dX[2]; // [0-1]: dx2-dxTa

  // get the positions of all the hits //
  ETFTrackTa *trk[2] = {t0, t1};
  vector<double> z[2], x[2], r[2]; // [0-1]: [preTaTrk, postTaTrk]
  for(int I = 0; I < 2; I++){ // loop over preTaTrk and postTaTrk
    z[I].resize(nh[I]); x[I].resize(nh[I]); r[I].resize(nh[I]);
    int i = 0;
    for(auto &p : trk[I]->GetHits()){
      p.second->GetProjZXR(z[I][i], x[I][i], r[I][i]);
      i++;
    } // end for
  } // end for over I
  // zt, xt: wirePos+r
  vector<double> zt[2], xt[2], dx[2]; // dx: the sigma of x
  for(int i = 2; i--;){
    zt[i].resize(nh[i]); zt[i].assign(zt[i].size(), -999.);
    xt[i].resize(nh[i]); xt[i].assign(xt[i].size(), -999.);
    dx[i].resize(nh[i]); dx[i].assign(dx[i].size(), -1.); // minus -> sigma not known
  } // end for over i
  for(int i = 2; i--;){
    LinearFit(z[i].data(), x[i].data(), dx[i].data(), nh[i], bl[i], kl[i], d2[i]);
    trk[i]->SetD2(d2[i]);
  } // end if
  double cosTheta[2], sinTheta[2];
  for(int i = 0; i < 2; i++){
    cosTheta[i] = 1. / sqrt(1.+kl[i]*kl[i]);
    sinTheta[i] = kl[i] * cosTheta[i];
  } // end for over i
  // assign signs for X1-X2 bothly fired wires //
  int singleLayer[2] = {-1, -1}; // ids (vector subscripts) of the single layers
  for(int I = 0; I < 2; I++){ // loop over preTaTrk-postTaTrk
    for(int i = 0; i < 2; i++){ // loop over DCTa0-1
      const int ss = I*4 + i*2;
      if(!(*trk[I])[ss] || !(*trk[I])[ss+1]){ // find singly fired wires
        // singleLayer is 2 if the second pdc in the pdc array has a singleLayer
        // because the first 2 layers are bothly fired, i.e., x[I][singleLayer] starts
        // at singleLayer = 2
        singleLayer[I] = i ? 2 : 0; // singly fired; i=1:(0,1)2; i=0:dc1, 0(1,2)
        continue; // singly fired PDC, no need for X1-X2 sign assignment
      } // end if
      //-- assign signs for bothly fired wires --//
      // note that vectors zt,xt,z,x and r all use hit-No (not layer Id) as subscripts //
      // find the subscript offset(s) for the bothly fired wires //
      // Let's think: i=0 => s=0; bothly fired layer in pdc0 (0,1)
      // i=1: singleLayer[I]=0 => s=1;  single layer in pdc0 (1,2)
      // i=1: singleLayer[I]=-1 => s=2; bothly fired layers in pdc0 and pdc1
      // -- (no singly fired wires)
      // i=1: singleLayer[I]=2: impossible, or we won't be here
      // -- (singly fired in pdc1, this part would be continued)
      const int s = i ? (singleLayer[I] ? 2 : 1) : 0; // offset for the subscripts
      // const double dz = fabs(z[I][s+1] - z[I][s]);
      // if(dz < 4. || dz > 15.) // to tell if s and s+1 belong to the same PDC
      //   er("ETFMath", "IterFit4Single: Peculiar, supposed to be adjacent X1X2");
      const bool sgn = x[I][s] > x[I][s+1]; // x0-x1 within a pdc
      // X1 //
      const double R0 = sgn ? r[I][s] : -r[I][s]; // the signed drift distance
      zt[I][s] = z[I][s] + R0*sinTheta[I];
      xt[I][s] = x[I][s] - R0*cosTheta[I];
      // X2 //
      const double R1 = sgn ? r[I][s+1] : -r[I][s+1]; // the signed drift distance
      zt[I][s+1] = z[I][s+1] - R1*sinTheta[I];
      xt[I][s+1] = x[I][s+1] + R1*cosTheta[I];
    } // end for over i
  } // end for over I
  // assign signs for X1 or X2 singly fired wires //
  for(int i = 0; i < n; i++){ // each i represents a set of r signs
    // assign signs to drift distances
    for(int I = 0; I < 2; I++){ // loop over preTaTrk-postTaTrk
      if(nSingle[I]){
        const int s = singleLayer[I];
        // the signed drift distance
        // const double R = r[I][s] * (2*((i>>(I*nSingle[0])) & 0x1)-1);
        const double R = (i>>(I*nSingle[0])) & 0x1 ? r[I][s] : -r[I][s];
        zt[I][s] = z[I][s] + R*sinTheta[I];
        xt[I][s] = x[I][s] - R*cosTheta[I];
      } // end if
      LinearFit(zt[I].data(), xt[I].data(), dx[I].data(), nh[I], bl[I], kl[I], d2[I]);
    } // end for over I
    d2T = d2[0] + d2[1] + Dx2DxTa_2(kl, bl, dX);
    if(d2T < d2Tmin){ // acknowledge the result
      t0->SetK(kl[0]); t0->SetB(bl[0]); t0->SetChi2(d2[0]);
      t1->SetK(kl[1]); t1->SetB(bl[1]); t1->SetChi2(d2[1]);
      fDx2 = dX[0]; fDxTa = dX[1];
      d2Tmin = d2T;
    } // end if
  } // end for over i

  return d2Tmin;
} // end member function IterFit4Single

// the sum of squares of dx2 and dxTa, used to calculte the additional term to
// the chi2 of the ameliorated tracking
// \param dx[0-1]: [dx2-dxTa]
double ETFMath::Dx2DxTa_2(double *k, double *b, double *dx){
  // calculate dx2 //
  double x2[2]{};
  rho(k[1], b[1], k[2], b[2], x2); // calculate rho in magField and obtain x2[2]
  dx[0] = x2[0] - x2[1];
  dx[0] -= -2.5; // an artifical shift to divert <dx2> to zero (for Jul.2018 exp)
  // dx[0] += 0.4; // an artifical shift to divert <dx2> to zero (for Dec.2023 exp)
  // calculate dxTa //
  static const double taZ = ETFDetectorManager::Instance()->GetTargetZ();
  dx[1] = (k[0] * taZ + b[0]) - (k[1] * taZ + b[1]); // dxTa
  // dx[1] += 1.443; // an artifical shift to divert <dxTa> to zero (for Dec.2023 exp)
  // now let's do the weighted sum //
  // 10.*k0/0.013 = 769*k0; 3 sigma(k0) scaled to 10 mm
  double dk = fabs(k[0]-k[1]), dktmp = 200. * dk; // 200->300, 2024-01-25
  if(dk > 0.02) dktmp *= 3.; // most likely a mismatch of preTaTrk and postTaTrk happened
  static const double kVdx2 = 10., kVdxTa = 2.5; // the relative variance V/sigma_DC
  return
    dx[0]*dx[0]   / kVdx2 // 10.
    + dx[1]*dx[1] / kVdxTa // 2.5
    + dktmp*dktmp // XXX 2020-03-12
    ;
} // end member function Dx2DxTa_2

// analytic PID method using DCTaArr
double ETFMath::rho(double k1, double b1, double k2, double b2, double *x2Arr,
  double *zo, double *xo){
  if(fabs(k1 - k2) < 1.e-8){
    ETFMsg::Warn("ETFMath", "rho: |k1-k2| too small: k1: %f, k2: %f", k1, k2);
    k2 = k1 + 1.e-7;
  } // end if

  const double z1 = kzMagIn,  x1 = k1 * z1 + b1; // incident point
  const double z2 = kzMagOut, x2 = k2 * z2 + b2; // exit point

  // x2 solved from equation L1 == L2
  const double x2p = x1+(z2-z1)/(k1+k2)*(k1*k2+sqrt(1.+k1*k1)*sqrt(1.+k2*k2)-1.);
  const double L2p = fabs(((z1-z2)+k1*(x1-x2p))/(k1-k2))*sqrt(1.+k2*k2);

  if(zo && xo){
    zo[0] = (k1*z2-k2*z1+k1*k2*(x2p-x1))/(k1-k2);
    xo[0] = (z1-zo[0])/k1+x1;
  } // end if

#ifdef DEBUG
// XXX: note that L2 is preferred, which agrees with L1p and L2p the better
  const double L1  = fabs(((z1-z2)+k2*(x1-x2 ))/(k1-k2))*sqrt(1.+k1*k1);
  const double L2  = fabs(((z1-z2)+k1*(x1-x2 ))/(k1-k2))*sqrt(1.+k2*k2);
  const double L1p = fabs(((z1-z2)+k2*(x1-x2p))/(k1-k2))*sqrt(1.+k1*k1);

  cout << "\n\nL1: " << L1 << "\tL2: " << L2; // DEBUG
  cout << "\nL1p: " << L1p << "\tL2p: " << L2p; // DEBUG
  cout << "\tx2: " << x2 << "\tx2p: " << x2p << endl; // DEBUG
  getchar(); // DEBUg
#endif

  if(x2Arr){
    x2Arr[0] = x2;
    x2Arr[1] = x2p;
  } // end if
  return L2p;
} // end member function rho

// \struct tSolve
// \brief The solution struct for solving particle trajectories in a uniform magnetic field.
struct tSolve{
  double x1, x2, ki, bi, zh, xh, dtheta, rho, zo, xo; // solution struct
};
// solve particle trajectory in uniform magnetic field, with only Mag boundry, exit track
// and target position known; returning the track radius of curvature in the magnetic field
// input unit: mm; output unit: mm
// x=kiz+bi, track before Target
// result: [0-7]: dtheta, rho, (ki, bi), (zo, xo), (x1, x2)
// dtheta: deflection angle; rho, (zo, xo): defines the arc;
// (ki, bi): defines the incident line; (x1, x2): x's of points of into and from magField
// zMagOut->z2; zMagIn->z1; zTa->z0; xTa->x0
bool ETFMath::UniformMagneticSolution(double k1, double b1, double z2, double z1,
    double z0, double x0, double *result){
  const double x2 = k1*z2+b1;
  if(fabs(x2) > 900.) return false; // impossible

  tSolve s[3]; // the equation is of 4-th order, having 4 solutions, one is dropped
  memset(s, 0, sizeof(s));
  for(auto &p : s) p.x2 = x2;

  // transform the equation to a simpler form: x1^3 + b x1^2 + c x1 + d = 0
  const double b = -2.*b1 - x0 - k1*(z0+z1);
  const double c = -x2*x2 + 2.*b1*(x0+x2) + 2.*k1*x2*z0 + z1*(2.*k1*x0-2.*z0+z1) + z2*(2.*z0-z2);
  const double d = -2.*b1*(x0*x2-(z0-z1)*(z1-z2)) +
    x0*(x2*x2-2.*k1*x2*z1+(z1-z2)*(z1-z2)) - k1*(z0-z1)*(x2*x2-z1*z1+z2*z2);

  // let x1 = y - b/3, then the original equation transforms to: y^3 + py + q = 0
  const double p = c - b*b/3.;
  const double q = d + 2.*b*b*b/27. - b*c/3.;

  // solve y^3 + py + q = 0 using Cardano's formula:
  const double qh = q/2., pt = p/3.;
  cdouble Delta(qh*qh + pt*pt*pt, 0.);
  cdouble sqrtDelta = sqrt(Delta);
  cdouble omega(-0.5, Sqrt3()/2.);
  cdouble cqh(qh, 0.);

  cdouble alphap = pow(-cqh + sqrtDelta, 1./3.);
  cdouble alpham = pow(-cqh - sqrtDelta, 1./3.);

  cdouble y[3];
  y[0] = alphap + alpham;
  y[1] = omega*alphap + omega*omega*alpham;
  y[2] = omega*omega*alphap + omega*alpham;

  // output the results and assign its geometrical meaning //
  for(int i = 0; i < 3; i++){
    if(fabs(y[i].imag()) < 1.E-7) s[i].x1 = y[i].real() - b/3.;
    else s[i].x1 = -999.; // not a real number
    if(fabs(s[i].x1) > 600.) s[i].x1 = -999.; // out of Mag mouth (+-500)
    if(-999. == s[i].x1) continue;
    s[i].ki = (s[i].x1 - x0)/(z1 - z0);
    if(fabs(s[i].ki) > 1.45){ s[i].x1 = -999.; continue; }
    s[i].bi = x0 - s[i].ki*z0;
    s[i].zh = (-b1 + s[i].bi)/(k1 - s[i].ki);
    s[i].xh = k1*s[i].zh + b1;
    s[i].dtheta = atan(k1) - atan(s[i].ki); // anti-clockwise deflection angle
    if(fabs(s[i].dtheta) > 150.*DEGREE()){ s[i].x1 = -999.; continue; }
    // P1Ph or PhP2 (pdf in misc/tools/math/UniBSolu)
    // if(fabs(s[i].rho) < 500. || fabs(s[i].rho) > 100000.){ s[i].x1 = -999.; continue; }
    s[i].zo = (k1*(s[i].ki*(s[i].x1 - x2) + z1) - s[i].ki*z2) / (k1 - s[i].ki);
    s[i].xo = (-s[i].ki*s[i].x1 + k1*x2 - z1 + z2) / (k1 - s[i].ki);
    s[i].rho = Distance(s[i].zo, s[i].xo, z2, s[i].x2) * ETFMath::sign(s[i].dtheta);
    // check if (zo, xo)->(z1, x1) == rho and (zo, xo)->(z2, x2) //
    const double r1 = Distance(s[i].zo, s[i].xo, z1, s[i].x1);
    const double r2 = Distance(z2, x2, s[i].zh, s[i].xh)/tan(s[i].dtheta/2.);
    const double r3 = DistanceToLine(s[i].zo, s[i].xo, k1, b1);
    // cout << "rho: " << s[i].rho << " r1: " << r1 <<" r2: " << r2 << " r3: " << r3 << endl; // DEBUG
    // getchar(); // DEBUG
    if(!Within(fabs(r1/s[i].rho), 0.95, 1.05) ||
       !Within(fabs(r2/s[i].rho), 0.95, 1.05) ||
       !Within(fabs(r3/s[i].rho), 0.95, 1.05)){
      s[i].x1 = -999.; continue;
    } // end if


// #define DEBUG_UNI
#ifdef DEBUG_UNI
    const double zo = s[i].zo, xo = s[i].xo; // DEBUG
    const double zh = s[i].zh, xh = s[i].xh; // DEBUG
    const double x1 = s[i].x1; // DEBUG
    const double dd_1 = sqrt(Sum2(z1-zh, x1-xh)); // DEBUG
    const double dd_2 = sqrt(Sum2(z2-zh, x2-xh)); // DEBUG
    const double rho_1 = sqrt(Sum2(z1-zo, x1-xo)); // DEBUG
    const double rho_2 = sqrt(Sum2(z2-zo, x2-xo)); // DEBUG
    cout << "dd_1: " << dd_1 << "\trho_1: " << rho_1 << endl;
    cout << "dd_2: " << dd_2 << "\trho_2: " << rho_2 << endl; // DEBUG
    getchar(); // DEBUG
#endif
  } // end for


  // solution validity check
  int validCnt = 0;
  for(const tSolve &t : s) if(-999. != t.x1) validCnt++;
  if(1 != validCnt){
    for(int i = 0; i < 6; i++) result[i] = validCnt;
    return false;
  } // end if(1 != validCnt)
  for(const tSolve &t : s) if(-999. != t.x1){
    // output the result
    result[0] = t.dtheta; result[1] = t.rho;
    result[2] = t.ki; result[3] = t.bi;
    result[4] = t.zo; result[5] = t.xo;
    result[6] = t.x1; result[7] = t.x2;
  } // end for
  return true;
} // end of function UniformMagneticSolution


double ETFMath::BetaGamma(double beta){
  if(beta < 0. || beta >= 1.){
    // er("ETFMath", "BetaGamma: input beta invalid: %f", beta);
    return -999.;
  } // end if
  return beta/sqrt(1.-beta*beta);
} // end member function BetaGamma

// Given a set of data x[0...ndata-1] and y[0...ndata-1] with standard deviations
// sigy[0...ndata-1]. Fit them to a straight line y(x) = a + bx by minimizing chi2.
// Returning a and b, and their respective probable uncertainties.siga and sigb, the
// chi-square chi2 and the goodness-of-fit probability q (that the fit would
// have chi2 this large or larger under the model given by the fitting). If
// dy[0] <= 0., then the standard deviations are assumed to be unavailable: q
// is returned as 1.0 and the normalization of chi2 is to unit standard deviation
// on all points.
// Ref. Numerical Recipes in C, p665.
void ETFMath::LinearFit(const double *x, const double *y, const double *dy, int ndata,
    double &a, double &b, double &chi2, double *siga, double *sigb, double *q){
  // stt=\sum_i{1/sigi*(xi-sx/s), sx=\sum_i{xi/sigi^2}, sy=\sum_i{yi/sigi^2}
  double s = 0., sx = 0., sy = 0., stt = 0.;
  const bool isSigYKnown = dy[0] > 0.;
  // accumulate sums ...
  if(isSigYKnown){
    s = 0.;
    for(int i = 0; i < ndata; i++){ // ...with weights
      const double wt = 1. / (dy[i]*dy[i]); // weights
      s  += wt;
      sx += wt*x[i];
      sy += wt*y[i];
    } // end for
  } // end if
  else{ // ...or without weights
    for(int i = 0; i < ndata; i++){
      sx += x[i];
      sy += y[i];
    } // end for over i
    s = ndata;
  } // end else
  const double sxos = sx / s; // sx over s
  // calculate the slope b
  b = 0.;
  if(isSigYKnown){
    for(int i = 0; i < ndata; i++){
      const double t = (x[i] - sxos)/dy[i];
      stt += t*t;
      b += t*y[i]/dy[i];
    } // end for
  } // end if
  else{
    for(int i = 0; i < ndata; i++){
      const double t = x[i] - sxos;
      stt += t*t;
      b += t*y[i];
    } // end for over i
  } // end else

  // solve for a, b, siga and sigb
  b /= stt;
  a = (sy-sx*b)/s;
  if(siga) *siga = sqrt((1.+sx*sx/(s*stt))/s);
  if(sigb) *sigb = sqrt(1./stt);

  // calculte chi2
  chi2 = 0.;
  const double nu = ndata - 2; // degree of freedom for chi2 distribution
  if(isSigYKnown){
    for(int i = 0; i < ndata; i++) chi2 += sqr((y[i]-a-b*x[i])/dy[i]);
    if(q) *q = ETFMath::gammaq(0.5*nu, 0.5*chi2);
  } // end if
  // for unweighted data, evaluate typical sig (which is assumed unit before)
  // using chi2, then update siga and sigb
  else{
    for(int i = 0; i < ndata; i++) chi2 += sqr(y[i]-a-b*x[i]);
    if(siga || sigb){
      // chi2~nu, chi2*sig^2=\sum{{yi-y(xi)}^2}~nu*sig^2
      const double sigData = sqrt(chi2 / nu);
      if(siga) *siga *= sigData;
      if(sigb) *sigb *= sigData;
    } // end if
  } // end else

  chi2 /= 1.+b*b; // so that chi2 is sum{d^2}, where d is distance_to_line
} // end member function LinearFit

//// functions serving 3D tracking ///
/* Mathemathica codes for solving UV->X and UV->Y
tu = -30 Degree;
tv = 30 Degree;
Ry[x_] := {{Cos[x], 0, Sin[x]}, {0, 1, 0}, {-Sin[x], 0, Cos[x]}};
Rz[x_] := {{Cos[x], -Sin[x], 0}, {Sin[x], Cos[x], 0}, {0, 0, 1}};
{xu, yu, zu} = Rz[-tu].Ry[-p].{x, y, z};
{xv, yv, zv} = Rz[-tv].Ry[-p].{x, y, z};
Collect[FullSimplify[Solve[Eliminate[{xu == ku zu + bu, xv == kv zv + bv}, y], x]], z] (*UV\[Rule]X*)
Collect[FullSimplify[Solve[Eliminate[{xu == ku zu + bu, xv == kv zv + bv}, x], y]], z] (*UV\[Rule]Y*)
*/
// Oxuyuzu = Rz[-theta].Ry[-phi].Oxyz: detector is along the zu axis and U wire is parallel to y axis
// Similar rotation has been implemented for V tracking
// U+V->X tranformation: lx: x=kz+b: slope
double ETFMath::kUV_X(double phi, double ku, double kv){
  return ((ku+kv)*cos(phi)+Sqrt3()*sin(phi))/(Sqrt3()*cos(phi)-(ku+kv)*sin(phi));
}
// U+V->X tranformation: lx: x=kz+b: intercept
double ETFMath::bUV_X(double phi, double ku, double kv, double bu, double bv){
  return (bu+bv)/(Sqrt3()*cos(phi)-(ku+kv)*sin(phi));
}
// U+V->Y tranformation: ly: y=kz+b: slope
double ETFMath::kUV_Y(double phi, double ku, double kv){
  return Sqrt3()*(-ku+kv)/(Sqrt3()*cos(phi)-(ku+kv)*sin(phi));
}
// U+V->Y tranformation: ly: y=kz+b: intercept
double ETFMath::bUV_Y(double phi, double ku, double kv, double bu, double bv){
  return (Sqrt3()*(-bu+bv)*cos(phi)+2.*(-bv*ku+bu*kv)*sin(phi))/(Sqrt3()*cos(phi)-(ku+kv)*sin(phi));
}
// X+Y->U tranformation: lu: xu=k zu+bu: slope
double ETFMath::kXY_U(double phi, double k1, double k2){
  return (-k2+Sqrt3()*(k1*cos(phi)-sin(phi)))/(2.*(cos(phi)+k1*sin(phi)));
}
// X+Y->U tranformation: lu: xu=k zu+bu: intercept
double ETFMath::bXY_U(double phi, double k1, double k2, double b1, double b2){
  return 0.5*(-b2+b1*(Sqrt3()+k2*sin(phi))/(cos(phi)+k1*sin(phi)));
}
// X+Y->V tranformation: lv: yv=k zv+bv: slope
double ETFMath::kXY_V(double phi, double k1, double k2){
  return (k2+Sqrt3()*(k1*cos(phi)-sin(phi)))/(2.*(cos(phi)+k1*sin(phi)));
}
// X+Y->V tranformation: lv: yv=k zv+bv: intercept
double ETFMath::bXY_V(double phi, double k1, double k2, double b1, double b2){
  return 0.5*(b2+b1*(Sqrt3()-k2*sin(phi))/(cos(phi)+k1*sin(phi)));
}

// calulate DC position (pos) and orientation (ang) from certain measurement
// points. The lengths are in mm. The ang are Euler angles in deg
void ETFMath::Pos(double *pos, double *ang, const double *a, const double *b,
    const double *c, const double *d, bool isDC1){
  // b's coordinate in DC-local frame; Z0U,D: upstream and downstream
  static const double X0 = 353., Y0 = 375., Z0U = -60. - DPAT, Z0D = 70. + DPAT;

  // coordinate of a, b, c, d in DC-local frame //
  const double Z0 = isDC1 ? Z0D : Z0U;
  const double a0[3] = {-X0,  Y0, Z0};
  const double b0[3] = { X0,  Y0, Z0};
  const double c0[3] = {-X0, -Y0, Z0};
  const double d0[3] = { X0, -Y0, Z0};

  // calculate the coordinates of the local-origin - i.e. X0's center //
  // plane Pi is defined by a0, b0, c0 and d0 - calculate its center
  for(int i = 3; i--;) pos[i] = (a[i] + b[i] + c[i] + d[i]) / 4.;
  // calculate the (normalized) normal vector of Pi //
  double n[3]{}, n0[3]{};
  Plane(n0, a, b, c); for(int i = 3; i--;) n[i] += n0[i];
  Plane(n0, a, b, d); for(int i = 3; i--;) n[i] += n0[i];
  Plane(n0, a, c, d); for(int i = 3; i--;) n[i] += n0[i];
  Plane(n0, b, c, d); for(int i = 3; i--;) n[i] += n0[i];
  const double nn = norm(n, 3) * sign(n[2]); // sign(nz): so that n -> beam
  for(int i = 3; i--;) n[i] /= nn; // normalize
  // return from center of Pi to origin along the normal vector n //
  for(int i = 3; i--;) pos[i] -= n[i] * Z0; // here we get the coord. of the local-frame
  // now we solve the orientatioin angles (yaw, pitch, row) //
  // clean off the translational offset
  double a1[3]{}, b1[3]{}, c1[3]{}, d1[3]{};
  for(int i = 3; i--;){
    a1[i] = a[i] - pos[i];
    b1[i] = b[i] - pos[i];
    c1[i] = c[i] - pos[i];
    d1[i] = d[i] - pos[i];
  } // end for
  // solve the rotation matrix //
  double r[3][3]{}, r0[3][3]{};
  SolveMatrix3(r0, a1, b1, c1, a0, b0, c0);
  for(int i = 3; i--;) for(int j = 3; j--;) r[i][j] += r0[i][j];
  SolveMatrix3(r0, a1, b1, d1, a0, b0, d0);
  for(int i = 3; i--;) for(int j = 3; j--;) r[i][j] += r0[i][j];
  SolveMatrix3(r0, a1, c1, d1, a0, c0, d0);
  for(int i = 3; i--;) for(int j = 3; j--;) r[i][j] += r0[i][j];
  SolveMatrix3(r0, b1, c1, d1, b0, c0, d0);
  for(int i = 3; i--;) for(int j = 3; j--;) r[i][j] += r0[i][j];
  for(int i = 3; i--;) for(int j = 3; j--;) r[i][j] /= 4.;
  // solve the trigonometric functions of the angles //
  const double s1 = -r[1][2], c1_ = sqrt(1. - s1*s1);
  const double s2 = r[1][0] / c1_, s0 = r[0][2] / c1_;

  ang[0] = asin(s0) / DEGREE();
  ang[1] = asin(s1) / DEGREE();
  ang[2] = asin(s2) / DEGREE();
  // DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG //
  // const double c0_ = r[2][2] / c1_, c2 = r[1][1] / c1_;
  // cout << "r[0][0] - (s0*s1*s2+c0*c2): " << r[0][0] - (c0_*c2+s0*s1*s2) << endl;
  // cout << "r[0][1] - (s0*s1*c2-c0*s2): " << r[0][1] - (s0*s1*c2-c0_*s2) << endl;
  // cout << "r[2][0] - (c0*s1*s2-s0*c2): " << r[2][0] - (c0_*s1*s2-s0*c2) << endl;
  // cout << "r[2][1] - (c0*s1*c2+s0*s2): " << r[2][1] - (c0_*s1*c2+s0*s2) << endl;
  // DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG DEBUG //
} // end member function Pos

// from pos calculate the four point a, b, c and d. The ang are Euler angles in deg
void ETFMath::InvPos(double *a, double *b, double *c, double *d, const double *pos,
    const double *ang, bool isDC1){
  // the four pin hole positions //
  // b's coordinate in DC-local frame; Z0U,D: upstream and downstream
  static const double X0 = 353., Y0 = 375., Z0U = -60. - DPAT, Z0D = 70. + DPAT;

  // coordinate of a, b, c, d in DC-local frame //
  const double Z0 = isDC1 ? Z0D : Z0U;
  const double a0[3] = {-X0,  Y0, Z0};
  const double b0[3] = { X0,  Y0, Z0};
  const double c0[3] = {-X0, -Y0, Z0};
  const double d0[3] = { X0, -Y0, Z0};

  const double angle[3] = {ang[0]*DEGREE(), ang[1]*DEGREE(), ang[2]*DEGREE()};
  rotate(a, a0, angle);
  rotate(b, b0, angle);
  rotate(c, c0, angle);
  rotate(d, d0, angle);
  for(int i = 3; i--;){
    a[i] += pos[i];
    b[i] += pos[i];
    c[i] += pos[i];
    d[i] += pos[i];
  } // end for over i
} // end member function InvPos

// calculate plane defined by a, b and c; return result to its normal vector n
// and constant to the return value of the function
// suppose plane function: Ax+By+Cz+1=0
double ETFMath::Plane(double *n, const double *a, const double *b, const double *c){
  const double ax = a[0], ay = a[1], az = a[2];
  const double bx = b[0], by = b[1], bz = b[2];
  const double cx = c[0], cy = c[1], cz = c[2];
  // calculate the normal vector of Pi using the following Mathematica codes
  // P[x_, y_, z_] := aa x + bb y + cc z + 1;
  // Solve[P[ax, ay, az] == 0 && P[bx, by, bz] == 0 && P[cx, cy, cz] == 0, {aa, bb, cc}]
  const double deno = az*by*cx-ay*bz*cx-az*bx*cy+ax*bz*cy+ay*bx*cz-ax*by*cz;
  if(fabs(deno) < 1.E-200)
    er("ETFMath", "Plane: the denominator is zero: %f", deno);
  n[0] = (-az*by+ay*bz+az*cy-bz*cy-ay*cz+by*cz)/deno;
  n[1] = ( az*bx-ax*bz-az*cx+bz*cx+ax*cz-bx*cz)/deno;
  n[2] = (-ay*bx+ax*by+ay*cx-by*cx-ax*cy+bx*cy)/deno;

  // we have assumed that the plane does not cross the origin, so the constant
  // item is predefined as 1.
  return 1.;
} // end member function Plane

// Rr = {{r00, r01, r02}, {r10, r11, r12}, {r20, r21, r22}};
// Solve[Rr.a0 == a && Rr.b0 == b && Rr.c0 == c, {r00, r01, r02, r10, 
//   r11, r12, r20, r21, r22}]
void ETFMath::SolveMatrix3(double r[][3], const double *a, const double *b,
    const double *c, const double *a0, const double *b0, const double *c0){
  const double ax = a[0], ay = a[1], az = a[2];
  const double bx = b[0], by = b[1], bz = b[2];
  const double cx = c[0], cy = c[1], cz = c[2];
  const double a0x = a0[0], a0y = a0[1], a0z = a0[2];
  const double b0x = b0[0], b0y = b0[1], b0z = b0[2];
  const double c0x = c0[0], c0y = c0[1], c0z = c0[2];
  const double deno =
    -(a0z*b0y*c0x-a0y*b0z*c0x-a0z*b0x*c0y+a0x*b0z*c0y+a0y*b0x*c0z-a0x*b0y*c0z);
  if(fabs(deno) < 1.E-200)
    er("ETFMath", "SolveMatrix3: the denominator is zero");

  r[0][0] = (-ax*b0z*c0y+a0z*bx*c0y+ax*b0y*c0z-a0y*bx*c0z-a0z*b0y*cx+a0y*b0z*cx)/deno;
  r[0][1] = ( ax*b0z*c0x-a0z*bx*c0x-ax*b0x*c0z+a0x*bx*c0z+a0z*b0x*cx-a0x*b0z*cx)/deno;
  r[0][2] = (-ax*b0y*c0x+a0y*bx*c0x+ax*b0x*c0y-a0x*bx*c0y-a0y*b0x*cx+a0x*b0y*cx)/deno;
  r[1][0] = (-ay*b0z*c0y+a0z*by*c0y+ay*b0y*c0z-a0y*by*c0z-a0z*b0y*cy+a0y*b0z*cy)/deno;
  r[1][1] = ( ay*b0z*c0x-a0z*by*c0x-ay*b0x*c0z+a0x*by*c0z+a0z*b0x*cy-a0x*b0z*cy)/deno;
  r[1][2] = (-ay*b0y*c0x+a0y*by*c0x+ay*b0x*c0y-a0x*by*c0y-a0y*b0x*cy+a0x*b0y*cy)/deno;
  r[2][0] = (-az*b0z*c0y+a0z*bz*c0y+az*b0y*c0z-a0y*bz*c0z-a0z*b0y*cz+a0y*b0z*cz)/deno;
  r[2][1] = ( az*b0z*c0x-a0z*bz*c0x-az*b0x*c0z+a0x*bz*c0z+a0z*b0x*cz-a0x*b0z*cz)/deno;
  r[2][2] = (-az*b0y*c0x+a0y*bz*c0x+az*b0x*c0y-a0x*bz*c0y-a0y*b0x*cz+a0x*b0y*cz)/deno;
} // end member function SolveMatrix3

// calculate distance between two parallel planes; PI: Ax+By+Cz+1=0, n=(A,B,C)
double ETFMath::PlaneDistance(const double *n0, const double *n1){
  double n0_[3]{}, n1_[3]{};
  const double n0n = norm(n0), n1n = norm(n1);
  for(int i = 3; i--;){ n0_[i] = n0[i] / n0n; n1_[i] = n1[i] / n1n; } // normalize
  cout << "n0: (" << n0_[0] << ", " << n0_[1] << ", " << n0_[2] << ")" << endl; // DEBUG
  cout << "n1: (" << n1_[0] << ", " << n1_[1] << ", " << n1_[2] << ")" << endl; // DEBUG
  cout << "n0n: " << n0n << ", n1n: " << n1n << endl; // DEBUG
  double pd0 = fabs(innerProd(n0_, n1_, 3)); // cos<n0, n1>
  const double pd = pd0 > 1. ? 1. : pd0; // due to roundoff error
  const double ang = acos(pd) / DEGREE();
  cout << "cos<n0, n1>: " << pd0 << ", <n0, n1> (in deg): " << ang << endl; // DEBUG
  if(ang > 5.){ // check parallelism
    er("ETFMath", "PlaneDistance: The 2 planes aren't parallel, ang: %f", ang);
    exit(1);
  } // end if
  return (n1n - n0n) / (n0n * n1n);
} // end member function PlaneDistance

void ETFMath::normalize(double *p, int n){
  const double nn = norm(p, n);
  if(nn < 1.E-200){ er("ETFMath", "normalize: zero denominator = %f", nn); exit(1); }
  for(int i = n; i--;) p[i] /= nn; // normalize
} // end member function normalize

// linear interpolation
// kk stores the slope upon request
double ETFMath::lii(double x0, double y0, double x1, double y1, double x, double *kk){
  // if(fabs(x0-x1) < fabs(y0-y1)*1.e-15) er("ETFMath", "lii: the denominator too small");
  const double k = (y1-y0)/(x1-x0);
  if(kk) *kk = k;
  if(fabs(x - x0) < fabs(x - x1)) return k*(x-x0) + y0;
  else return k*(x-x1) + y1;
} // end member function lii

long ETFMath::filesize(const string &f){
  if(access(f.data(), F_OK)){
    ETFMsg::Warn("ETFMath", "filesize: %s does not exist", f.data());
    return 0;
  } // end if
  FILE *file = std::fopen(f.data(), "rb");
  if(file){
    std::fseek(file, 0, SEEK_END);
    long size = std::ftell(file);
    std::fclose(file);
    return size;
  } // end if
  return 0;
} // end member function filesize

double ETFMath::filesizeInMB(const string &f){
  if(access(f.data(), F_OK)){
    ETFMsg::Warn("ETFMath", "filesizeInMB: %s does not exist", f.data());
    return 0;
  } // end if
  return filesize(f) / MB();
} // end member function filesize
